#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

static const std::string OPENCV_WINDOW = "Image window";


std::string topic_id;
std::string node_name;
std::string img_type;

// sensor_msgs::ImageConstPtr ----> this_node---->CV image process ----> publish 
//                                           |
//                           cv::imshow()<----

int getParam(ros::NodeHandle nh)
{
    node_name = ros::this_node::getName();

    std::cout<<node_name<<std::endl;

    nh.param<std::string>(node_name+"/topic_id", topic_id,"");
    nh.param<std::string>(node_name+"/img_type", img_type,"color");

    return 0;
}

class ImageConverter
{
  ros::NodeHandle nh_;
  image_transport::ImageTransport it_;
  image_transport::Subscriber image_sub_;
  image_transport::Publisher image_pub_;

  public:
  ImageConverter(): it_(nh_){
        
    getParam(nh_);
    
    image_sub_ = it_.subscribe(topic_id, 1, &ImageConverter::imageCb, this);

    image_pub_ = it_.advertise(node_name + '/'+img_type, 1);

    cv::namedWindow(OPENCV_WINDOW);
  }

  ~ImageConverter()
  {
    cv::destroyWindow(OPENCV_WINDOW);
  }

  void imageCb(const sensor_msgs::ImageConstPtr& msg)
  {

    cv_bridge::CvImagePtr cv_ptr;
    try
    {
      if(img_type == "color")
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
      else if(img_type == "depth")
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::MONO16);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }
    // Draw an example circle on the video stream
    // Update GUI Window

    if(img_type == "depth"){

      cv::Mat eight_bit_img = cv::Mat::zeros(cv_ptr->image.size(), CV_8UC1);

      cv::Mat color_map_img;

      cv::convertScaleAbs(cv_ptr->image,eight_bit_img);

      cv::applyColorMap(eight_bit_img, color_map_img,cv::COLORMAP_JET);

      cv::imshow(OPENCV_WINDOW, color_map_img);
    }
    else if(img_type == "color")
      cv::imshow(OPENCV_WINDOW, cv_ptr->image);

    cv::waitKey(3);

    // Output modified video stream
    image_pub_.publish(cv_ptr->toImageMsg());

  }

  uint32_t getNumPublishers(){

    return image_sub_.getNumPublishers();

  }

};

int main(int argc, char** argv)
{
  // "empty" nodename is a placeholder for launch file node_name
  ros::init(argc, argv, "empty");

  ImageConverter ic;

  while(true){
    ros::spinOnce();
    if(!ic.getNumPublishers()){
      ROS_INFO("waiting for message");
      while(!ic.getNumPublishers())
      {
        //just wait
      }
    }
  }

  return 0;
}